/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/*LINTLIBRARY*/
/*PROTOLIB1*/

#include "main.h"
#include "celpfilt.h"
#include "adapt.h"
#include "submult.h"
#include "del_tab.h"
#include "acb_parm.h"
#include "delay.h"
#include "bwexp.h"
#include "zero_array16.h"
#include "acb_code.h"
#include "move_array16.h"
#include "con_adap.h"
/*#include <math.h>*/
#include <assert.h>
#include <string.h>

#ifdef INTG
int     fraction = FALSE;
int     neigh = FALSE;
#else /* INTG */
#ifdef FULL
int     fraction = TRUE;
int     neigh = FALSE;
#else /* FULL --
        if no compile-time options are specified, the hierarchical
        search below is used (FS1016 method) */
int     fraction = FALSE;
int     neigh = TRUE;
int     NeighborhoodRange = 3;
#endif /* FULL */
#endif /* INTG */

#define START           B_PTR - SF_LEN + 1



/* Length of truncated impulse response */
/* IR values past 30 are nearly zero */
/*
 *   #define LEN_TRUNC_H        30
 *
 * Value now defined in main.h
 *
 */
STATIC void CalcPointers(
int     even,
int     PrevDelayPtr,
int     *MinDelayPtr,
int     *MaxDelayPtr);

STATIC void TopMatch(
fxpt_32 match[MAX_NUM_ADAPT],
int     MinDelayPtr,
int     MaxDelayPtr,
int     *BestDelayPtr);

STATIC void SelectDelay(
int     *BestDelayPtr,
int     SubMult[256][4],
fxpt_32 match[MAX_NUM_ADAPT],
int     MinDelayPtr,
int     MaxDelayPtr);

STATIC void Neighborhood(
fxpt_16 lp_imp[SF_LEN],
fxpt_16 residual[RES_LEN],
fxpt_32 match[MAX_NUM_ADAPT],
int     MinDelayPtr,
int     MaxDelayPtr,
fxpt_16 DelayTable[],
fxpt_16 AdaptCB[MAX_ABUF_LEN],
int     *BestDelayPtr,
fxpt_32 gain[MAX_NUM_ADAPT]);

STATIC void CalcIntAdaptParms(
fxpt_16 AdaptCB[MAX_ABUF_LEN],
fxpt_16 pc_imp[SF_LEN],
int     MinDelayPtr,
int     MaxDelayPtr,
fxpt_16 DelayTable[MAX_NUM_ADAPT],
fxpt_16 residual[RES_LEN],
fxpt_32 gain[MAX_NUM_ADAPT],
fxpt_32 match[MAX_NUM_ADAPT]);

STATIC void CalcFracAdaptParms(
fxpt_16 AdaptCB[MAX_ABUF_LEN],
fxpt_16 pc_imp[SF_LEN],
int     MinDelayPtr,
int     MaxDelayPtr,
fxpt_16 DelayTable[MAX_NUM_ADAPT],
fxpt_16 residual[RES_LEN],
fxpt_32 gain[MAX_NUM_ADAPT],
fxpt_32 match[MAX_NUM_ADAPT]);

STATIC void FindAdaptResidual(
fxpt_16 speech[SF_LEN],
fxpt_16 AdaptCB[MAX_ABUF_LEN],
fxpt_16 pc[ORDER+1],
fxpt_16 AdaptGain,
fxpt_16 AdaptDelay,
fxpt_16 residual[RES_LEN]);

/**************************************************************************
*                                                                         *
* ROUTINE
*               AdaptiveAnalysis
*
* FUNCTION
*               Subframe adaptive codebook search
* SYNOPSIS
*               AdaptiveAnalysis(speech_in, pc, h, AdaptCB,
*                       residual_in, AdaptDelay, AdaptGain, residual_out)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       SpeechIn        fxpt_16  i      Subframe of speech
*       pc              fxpt_16  i      Predictor Coefficients (PCs)
*       pc_imp          fxpt_16  i      Impulse response of PCs
*       AdaptCB         fxpt_16  i      Adaptive codebook vector
*       residual_in     fxpt_16  i      Residual from LPC analysis
*       AdaptDelay      fxpt_16  o      pitch delay parameter to TX
*       AdaptGain       fxpt_16  o      pitch gain parameter to TX
*       residual_out    fxpt_16  o      residual remaining after comparison
*
**************************************************************************/

void AdaptiveAnalysis(
fxpt_16 SpeechIn[SF_LEN],                       /* 15.0 format */
fxpt_16 pc[ORDER],                              /* 2.13 format */
fxpt_16 pc_imp[SF_LEN],                         /* 2.13 format */
fxpt_16 AdaptCB[MAX_ABUF_LEN],                  /* 15.0 format */
fxpt_16 residual_in[RES_LEN],                   /* 15.0 format */
fxpt_16 *AdaptDelay,                            /* 8.7 format */
fxpt_16 *AdaptGain,                             /* 1.14 format */
fxpt_16 residual_out[RES_LEN])                  /* 15.0 format */
{
        fxpt_32         match[MAX_NUM_ADAPT];   /* 30.1 format */
        fxpt_32         gain[MAX_NUM_ADAPT];    /* 17.14 format */
        int             MaxDelayPtr, MinDelayPtr, BestDelayPtr;
        static  int     even=FALSE, PrevDelayPtr=1, first=TRUE;
        int             index;
        fxpt_16         TempAdaptCB[B_PTR];     /* 15.0 format */
#ifdef NEW_ACB
        int             i, Delay;
        fxpt_16         pcexp[ORDER + 1];
#endif

FXPT_PUSHSTATE("AdaptiveAnalysis", -1.0, -1.0);
        /* Update temporary adaptive codebook */
        ASSERT( B_PTR-(ACB_SIZE)-SF_LEN == 0 );

        ZeroArray16(TempAdaptCB + ACB_SIZE, B_PTR - ACB_SIZE);
        MoveArray16(&TempAdaptCB[B_PTR-(ACB_SIZE)-SF_LEN], AdaptCB, ACB_SIZE);

DUMPARR0(AdaptCB,ACB_SIZE)

        /* Set up initial conditions on first subframe */
        if (first) {
                first = FALSE;
                *AdaptGain = 0;
                *AdaptDelay = MIN_DELAY;
        }

        /* Calculate adaptive gain and delay for subsequent subframes */
        else {

                /* Determine allowable pointer range for full/delta
                 * searches on even/odd subframes
                 */
                CalcPointers(even, PrevDelayPtr, &MinDelayPtr, &MaxDelayPtr);

                /* Initialize arrays */
                memset(match, 0, MAX_NUM_ADAPT * sizeof(match[0]));
                memset(gain, 0, MAX_NUM_ADAPT * sizeof(gain[0]));
                // for (i=0; i<MAX_NUM_ADAPT; i++)
                //      match[i] = gain[i] = 0;

                /* Calculate gain and match scores for integer adaptive
                 * delays
                 */
                CalcIntAdaptParms(TempAdaptCB, pc_imp, MinDelayPtr,
                    MaxDelayPtr, DelayTable, residual_in, gain, match);

DUMPARR(gain,14,MAX_NUM_ADAPT)
DUMPARR(match,1,MAX_NUM_ADAPT)

                /* Calculate gain and match scores for fractional delays */
                if (fraction)
				{
                        CalcFracAdaptParms(TempAdaptCB, pc_imp, MinDelayPtr,
                            MaxDelayPtr, DelayTable, residual_in, gain, match);
DUMPARR(gain,14,MAX_NUM_ADAPT)
DUMPARR(match,1,MAX_NUM_ADAPT)

				}

                /* Determine top match score from those computed */
                TopMatch(match, MinDelayPtr, MaxDelayPtr, &BestDelayPtr);

LOGFIXED0(BestDelayPtr);

                /* Select shortest delay */
                if (!even)
                        SelectDelay(&BestDelayPtr, submult, match, MinDelayPtr,
                            MaxDelayPtr);

                /* Find best neighborhood match */
                if (neigh) {
                        Neighborhood(pc_imp, residual_in, match, MinDelayPtr,
                            MaxDelayPtr, DelayTable, TempAdaptCB,
                            &BestDelayPtr, gain);
                }

                /* Assign pitch (adaptive codebook) parameters */
                *AdaptGain = fxpt_saturate16(gain[BestDelayPtr]);
                *AdaptDelay = DelayTable[BestDelayPtr];

                /* Save pointer to detrmine delta delay */
                PrevDelayPtr = BestDelayPtr;

                /* Encode adaptive gain (quantize) */
                *AdaptGain = ACBGainEncode(*AdaptGain, &index);

#ifdef NEW_ACB
FXPT_PUSHSTATE("NEW_ACB", -1.0, -1.0);
                /* ACB contribution to LP Impulse Response */
                if (*AdaptDelay < fxpt_shl16_fast(SF_LEN, 7)) {
                        /* Lay out pulses at multiples of the pitch period */
                        Delay = fxpt_shr16_fast(*AdaptDelay, 7);
                        ZeroArray16(pc_imp, SF_LEN);
                        pc_imp[0] = 8192;       /* 1.0 in 2.13 format */
                        for (i=Delay; i<SF_LEN; i+=Delay)
                                pc_imp[i] = fxpt_shr16_round(*AdaptGain, 1);
                        /* Filter the new signal with expanded PCs
                         * (no history on filter)
                         */
                        BWExpand(GAMMA, pc, pcexp);
                        FilterImpulseResponse(pc_imp, pcexp);
                }
FXPT_POPSTATE();
#endif
        }

        /* Calculate residual after pitch analysis */
        MoveArray16(TempAdaptCB, AdaptCB, ACB_SIZE);
        ZeroArray16(TempAdaptCB + ACB_SIZE, B_PTR - ACB_SIZE);

        FindAdaptResidual(SpeechIn, TempAdaptCB, pc, *AdaptGain, *AdaptDelay,
            residual_out);

        /* Update values for next subframe */
        even = (even) ? FALSE : TRUE;
FXPT_POPSTATE();
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               CalcPointers
*
* FUNCTION
*               Calculate allowable pointer range
* SYNOPSIS
*               CalcPointers(even, oldptr, minptr, maxptr)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       even            int      i      Even/Odd subframe marker
*       PrevDelayPtr    int      i      Previous delay pointer
*       MinDelayPtr     int      o      Pointer to minimum delay
*       MaxDelayPtr     int      o      Pointer to maximum delay
*
**************************************************************************/
void CalcPointers(
int     even,
int     PrevDelayPtr,
int     *MinDelayPtr,
int     *MaxDelayPtr)
{
        /* Delta delay coding on even subframes */
        if (even) {
                *MinDelayPtr = PrevDelayPtr - (NUM_DELTA_ADELAYS / 2 - 1);
                *MaxDelayPtr = PrevDelayPtr + (NUM_DELTA_ADELAYS / 2);
                if (*MinDelayPtr < 0) {
                        *MinDelayPtr = 0;
                        *MaxDelayPtr = NUM_DELTA_ADELAYS - 1;
                }
                if (*MaxDelayPtr > NUM_FULL_ADELAYS-1)  {
                        *MaxDelayPtr = NUM_FULL_ADELAYS - 1;
                        *MinDelayPtr = NUM_FULL_ADELAYS - NUM_DELTA_ADELAYS;
                }
        }
        /* Full range coding on odd subframes */
        else {
                *MinDelayPtr = 0;
                *MaxDelayPtr = NUM_FULL_ADELAYS - 1;
        }
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               TopMatch
*
* FUNCTION
*               Find pointer to top (MSPE) match score (BestDelayPtr)
*               search for best match score (max -error term)
* SYNOPSIS
*               TopMatch(match, MinDelayPtr, MaxDelayPtr, BestDelayPtr)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       match           fxpt_32  i      Match score array
*       MinDelayPtr     int      i      Pointer to minimum delay to search
*       MaxDelayPtr     int      i      Pointer to maximum delay to search
*       BestDelayPtr    int      o      Pointer to best delay
*
**************************************************************************/

void TopMatch(
fxpt_32 match[MAX_NUM_ADAPT],
int     MinDelayPtr,
int     MaxDelayPtr,
int     *BestDelayPtr)
{
        int     i;
        fxpt_32 emax;

        *BestDelayPtr = MinDelayPtr;

        emax = match[*BestDelayPtr];

        for (i=MinDelayPtr; i<=MaxDelayPtr; i++) {
                if (match[i] > emax) {
                        *BestDelayPtr = i;
                        emax = match[*BestDelayPtr];
                }
        }
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               SelectDelay
*
* FUNCTION
*               Select shortest delay of 2, 3, or 4 submultiples
*               if its match is within 1 dB of MSPE to favor
                smooth "pitch"
* SYNOPSIS
*               SelectDelay(BestDelayPtr, SubMult, match,
*                             MinDelayPtr, MaxDelayPtr)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       BestDelayPtr    int     i/o     Pointer to delay with top match score
*       SubMult         int      i      Sumbmultiples array
*       match           fxpt_32  i      Match score array for each delay
*       MinDelayPtr     int      i      Pointer to minimum delay to search
*       MaxDelayPtr     int      i      Pointer to maximum delay to search
*
**************************************************************************/
void SelectDelay(
int     *BestDelayPtr,
int     SubMult[256][4],
fxpt_32 match[MAX_NUM_ADAPT],           /* 30.1 format */
int     MinDelayPtr,
int     MaxDelayPtr)
{
        int     i;
        int     SubMultPtr, BigPtr;
        int     Best;

FXPT_PUSHSTATE("SelectDelay", -1.0, -1.0);
        /* Initialize */
        Best = *BestDelayPtr;

        /* For each submultiple (2, 3, & 4) */
        for (i=1; i<=SubMult[*BestDelayPtr][0]; i++) {

                /* Find best neighborhood match for given submultiple */
                BigPtr = SubMult[*BestDelayPtr][i];
                for (SubMultPtr = max(SubMult[*BestDelayPtr][i]-8, MinDelayPtr);
                    SubMultPtr <= min(SubMult[*BestDelayPtr][i]+8, MaxDelayPtr);
                    SubMultPtr++) {
                        if (match[SubMultPtr] > match[BigPtr])
                                BigPtr = SubMultPtr;
                }

                /* Select submultiple match if within 1 dB MSPE match */
                /*if (match[BigPtr] >= (0.88*match[*BestDelayPtr]))*/
                if (match[BigPtr] >=
                    fxpt_mult64_fix(28836, match[*BestDelayPtr], 15))
                        Best = BigPtr;
        }

        *BestDelayPtr = Best;
FXPT_POPSTATE();
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               Neighborhood
*
* FUNCTION
*               Search BestDelayPtr's neighboring delays (to be used with
*               earlier stages of searching).  Find gain and match
*               score for neighboring delays and find best neighborhood
*               match (could use end-point correction on unity spaced delays)
* SYNOPSIS
*               Neighborhood(lp_imp, residual, match, MinDelayPtr, MaxDelayPtr
                                DelayTable, AdaptCB, BestDelayPtr, gain)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       lp_imp          fxpt_16  i      LP impulse response
*       residual        fxpt_16  i      Residual from LPC analysis
*       match           fxpt_32  i      Adaptive Codebook match score array
*       MinDelayPtr     int      i      Pointer to minimum delay to search
*       MaxDelayPtr     int      i      Pointer to maximum delay to search
*       DelayTable      fxpt_16  i      Table of integer and fractional delays
*       AdaptCB         fxpt_16  i      Adaptive codebook
*       BestDelayPtr    int      o      Pointer to best delay
*       gain            fxpt_16  o      Adaptive Codebook gain array
*
**************************************************************************/
void Neighborhood(
fxpt_16 lp_imp[SF_LEN],                 /* 15.0 format */
fxpt_16 residual[RES_LEN],              /* 2.13 format */
fxpt_32 match[MAX_NUM_ADAPT],           /* 30.1 format */
int     MinDelayPtr,
int     MaxDelayPtr,
fxpt_16 DelayTable[],                   /* 8.7 format */
fxpt_16 AdaptCB[MAX_ABUF_LEN],          /* 15.0 format */
int     *BestDelayPtr,
fxpt_32 gain[MAX_NUM_ADAPT])            /* 17.14 format */
{
        int     i;
        int     BigPtr;
        fxpt_16 IntDelay;
        fxpt_16 FracDelay;                      /* 8.7 format */
        fxpt_16 AdaptCBShift[MAX_A_LEN];        /* 15.0 format */

FXPT_PUSHSTATE("Neighborhood", -1.0, -1.0);
        BigPtr = *BestDelayPtr;

        for (i=max(*BestDelayPtr - NeighborhoodRange, MinDelayPtr);
            i<=min(*BestDelayPtr + NeighborhoodRange, MaxDelayPtr); i++) {
                if (i != *BestDelayPtr) {
                        IntDelay = fxpt_shr16_fast(DelayTable[i], 7);
                        FracDelay = fxpt_sub16(DelayTable[i],
                            fxpt_shl16_fast(IntDelay, 7));
                        if (FracDelay > 0) {
                                delay_short(&AdaptCB[START-1], FracDelay,
                                    IntDelay, &AdaptCBShift[0]);
                                CalcACBParms(AdaptCBShift, lp_imp,
                                    TRUE, 69, residual, &match[i],
                                    &gain[i]);
                        }
                        if (match[i] > match[*BestDelayPtr])
                                BigPtr = i;
                }
        }

        *BestDelayPtr = BigPtr;
FXPT_POPSTATE();
}

/**************************************************************************
*
* ROUTINE
*               CalcIntAdaptParms
*
* FUNCTION
*               Calculate gain and match score for integer adaptive delays
* SYNOPSIS
*               CalcIntAdaptParms(AdaptCB, pc_imp, MinDelayPtr, MaxDelayPtr,
*                   DelayTable, residual, gain, match)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       AdaptCB         fxpt_16  i      Adaptive Codebook
*       pc_imp          fxpt_16  i      Impulse response of PCs
*       MinDelayPtr     int      i      Pointer to minimum delay to search
*       MaxDelayPtr     int      i      Pointer to maximum delay to search
*       DelayTable      fxpt_16  i      Adaptive delay coding table
*       residual        fxpt_16  i      Residual from LPC analysis
*       gain            fxpt_32  o      Gain array
*       match           fxpt_32  o      Match score array
*
**************************************************************************
*
*       Uses end-point correction on unity spaced delays
*
**************************************************************************/

void CalcIntAdaptParms(
fxpt_16 AdaptCB[MAX_ABUF_LEN],          /* 15.0 format */
fxpt_16 pc_imp[SF_LEN],                 /* 2.13 format */
int     MinDelayPtr,
int     MaxDelayPtr,
fxpt_16 DelayTable[MAX_NUM_ADAPT],      /* 8.7 format */
fxpt_16 residual[RES_LEN],              /* 15.0 format */
fxpt_32 gain[MAX_NUM_ADAPT],            /* 17.14 format */
fxpt_32 match[MAX_NUM_ADAPT])           /* 30.1 format */
{
        int     first = TRUE;
        fxpt_16 IntDelay;
        fxpt_16 FracDelay;              /* 8.7 format */
        int     i;

FXPT_PUSHSTATE("CalcIntAdaptParms", -1.0, -1.0);
        for (i=MinDelayPtr; i<=MaxDelayPtr; i++) {
                IntDelay = (int) fxpt_shr16_fast(DelayTable[i], 7);

                /*FracDelay = DelayTable[i] - IntDelay;*/
                FracDelay = fxpt_sub16(DelayTable[i],
                    fxpt_shl16_fast(IntDelay, 7));
                if (FracDelay == 0) {
                        CalcACBParms(&AdaptCB[START - IntDelay-1],
                                     &pc_imp[0], first, IntDelay,
                                     &residual[0], &match[i], &gain[i]);
                        first = FALSE;
                }
                else
                        match[i] = 0;
        }
FXPT_POPSTATE();
}


/**************************************************************************
*                                                                         *
* ROUTINE
*               CalcFracAdaptParms
*
* FUNCTION
*               Calculate gain and match score for fractional adaptive delays
* SYNOPSIS
*               CalcFracAdaptParms(AdaptCB, pc_imp, MinDelayPtr, MaxDelayPtr,
*                       DelayTable, residual, gain, match)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       AdaptCB         fxpt_16  i      Adaptive Codebook
*       pc_imp          fxpt_16  i      Impulse response of PCs
*       MinDelayPtr     int      i      Minimum delay pointer to search
*       MaxDelayPtr     int      i      Maximum delay pointer to search
*       DelayTable      fxpt_16  i      Adaptive delay coding table
*       residual        fxpt_16  i      Residual from LPC analysis
*       gain            fxpt_32  o      Gain array
*       match           fxpt_32  o      Match score array
*
**************************************************************************
*
*       Uses end-point correction on unity spaced delays
*
**************************************************************************/
void CalcFracAdaptParms(
fxpt_16 AdaptCB[MAX_ABUF_LEN],                  /* 15.0 format */
fxpt_16 pc_imp[SF_LEN],                         /* 2.13 format */
int     MinDelayPtr,
int     MaxDelayPtr,
fxpt_16 DelayTable[MAX_NUM_ADAPT],              /* 8.7 format */
fxpt_16 residual[RES_LEN],                      /* 15.0 format */
fxpt_32 gain[MAX_NUM_ADAPT],                    /* 17.14 format */
fxpt_32 match[MAX_NUM_ADAPT])                   /* 30.1 format */
{
        fxpt_16 IntDelay;
        fxpt_16 FracDelay;                      /* 8.7 format */
        int     i;
        fxpt_16 AdaptCBShift[MAX_A_LEN];        /* 15.0 format */

FXPT_PUSHSTATE("CalcFracAdaptParms", -1.0, -1.0);
        for (i=MinDelayPtr; i<=MaxDelayPtr; i++) {
                IntDelay = fxpt_shr16_fast(DelayTable[i], 7);
                FracDelay = fxpt_sub16(DelayTable[i],
                    fxpt_shl16_fast(IntDelay, 7));
                if (FracDelay > 0) {
                        delay_short(&AdaptCB[START-1], FracDelay,
                            IntDelay, AdaptCBShift);
                        CalcACBParms(AdaptCBShift, pc_imp, 1, 69,
                                     residual, &match[i], &gain[i]);
                }
        }
FXPT_POPSTATE();
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               FindAdaptResidual
*
* FUNCTION
*               Find residual after adaptive analysis
* SYNOPSIS
*               FindAdaptResidual(speech, AdaptCB, pc, AdaptGain, AdaptDelay,
*                       residual)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       speech          fxpt_16  i      Subframe of input speech
*       AdaptCB         fxpt_16  i      Adaptive Codebook vector
*       pc              fxpt_16  i      Subframe of interpolated predictor
*                                       coefficients
*       AdaptGain       fxpt_16  i      Adaptive codebook gain
*       AdaptDelay      fxpt_16  i      Adaptive codebook delay
*       residual        fxpt_16  o      Adaptive residual
*
**************************************************************************/

void FindAdaptResidual(
fxpt_16 speech[SF_LEN],                 /* 15.0 format */
fxpt_16 AdaptCB[MAX_ABUF_LEN],          /* 15.0 format */
fxpt_16 pc[ORDER+1],                    /* 2.13 format */
fxpt_16 AdaptGain,                      /* 1.14 format */
fxpt_16 AdaptDelay,                     /* 8.7 format */
fxpt_16 residual[RES_LEN])              /* 15.0 format */
{
        int     i;
        fxpt_16 pcexp[ORDER+1];         /* 2.13 format */

FXPT_PUSHSTATE("FindAdaptResidual", -1.0, -1.0);
        /* Initialize residual */
        ZeroArray16(residual, RES_LEN);

        /* Adaptive Codebook Synthesis */
        ConstructAdaptCW(residual, AdaptCB, AdaptGain, AdaptDelay);

        /* LP Filter */
        do_pfilt_dynamic(&Adapt_ResP, pc, residual);

        /* Calculate Residual */
        for (i=0; i<RES_LEN; i++) {
                residual[i] = fxpt_sub16(speech[i], residual[i]);
        }

        /* Perceptual Weighting of residual */
        BWExpand(GAMMA, pc, pcexp);
        do_zpfilt_dynamic(&Adapt_ResZ, &Adapt_ResP2, pc, pcexp, residual);
FXPT_POPSTATE();
}
